#include "EvalUtil.h"
#include "SolverUtil.h"
#include "BEMUtil.h"
#include <surftrack.h>

extern SurfTrack* surf;

void myMatrixVectorProduct( double* x, double** y, int n )
{
	int nTri = n / 3;
	//double* A = (double*)malloc(3 * n * sizeof(double));

	vectorAmplititude(x, 0.5, y, n);
	//vectorClear(y, n);
	//vectorClear(&A, 3*n);

#pragma omp parallel for
	for (int i=0; i<nTri; i++)
	{
		Vec3ui ptri = surf->m_mesh.get_triangle(i);

		Vec3d p0 = surf->get_position(ptri[0]);
		Vec3d p1 = surf->get_position(ptri[1]);
		Vec3d p2 = surf->get_position(ptri[2]);

		Vec3d p = (p0 + p1 + p2) / 3.0;

		for (int j=0; j<nTri; j++)
		{
			Vec3ui qtri = surf->m_mesh.get_triangle(j);

			Vec3d q0 = surf->get_position(qtri[0]);
			Vec3d q1 = surf->get_position(qtri[1]);
			Vec3d q2 = surf->get_position(qtri[2]);

			double T = surf->get_triangle_area(j);

			Vec3d qn = surf->get_triangle_normal(j);

			Mat33d v0 = evaluateGradientQuadraturePoint(p, q0, qn);
			Mat33d v1 = evaluateGradientQuadraturePoint(p, q1, qn);
			Mat33d v2 = evaluateGradientQuadraturePoint(p, q2, qn);

			Vec3d vx(x[3*j+0], x[3*j+1], x[3*j+2]);
			Vec3d vy = (v0 + v1 + v2) * T / 3.0 * vx;

			(*y)[3*i+0] += vy[0];
			(*y)[3*i+1] += vy[1];
			(*y)[3*i+2] += vy[2];

		}
	}
}

void evaluateDisplacementField( double* theta, double** u )
{
	int nTri = surf->m_mesh.num_triangles();
	int nVer = surf->get_num_vertices();
	int n = nVer * 3;

	vectorClear(u, n);

#pragma omp parallel for
	for (int i=0; i<nVer; i++)
	{
		Vec3d p = surf->get_position(i);

		for (int j=0; j<nTri; j++)
		{
			Vec3ui qtri = surf->m_mesh.get_triangle(j);

			Vec3d q0 = surf->get_position(qtri[0]);
			Vec3d q1 = surf->get_position(qtri[1]);
			Vec3d q2 = surf->get_position(qtri[2]);

			double T = surf->get_triangle_area(j);

			Mat33d v0 = evaluateQuadraturePoint(p, q0);
			Mat33d v1 = evaluateQuadraturePoint(p, q1);
			Mat33d v2 = evaluateQuadraturePoint(p, q2);

			Vec3d vx(theta[3*j+0], theta[3*j+1], theta[3*j+2]);
			Vec3d vy = (v0 + v1 + v2) * T / 3.0 * vx;

			(*u)[3*i+0] += vy[0];
			(*u)[3*i+1] += vy[1];
			(*u)[3*i+2] += vy[2];
		}
	}
}

void evaluateDeformationGradientField( double* theta, double** F )
{
	int nTri = surf->m_mesh.num_triangles();
	int nVer = surf->get_num_vertices();
	int n = 9 * nVer;

	//vectorClear(F, n);

	double K[3][3][3];

#pragma omp parallel for
	for (int i=0; i<nVer; i++)
	{
		Vec3d p = surf->get_position(i);

		Mat33d Ft(0,0,0,0,0,0,0,0,0);

		for (int j=0; j<nTri; j++)
		{
			Vec3ui qtri = surf->m_mesh.get_triangle(j);

			Vec3d q0 = surf->get_position(qtri[0]);
			Vec3d q1 = surf->get_position(qtri[1]);
			Vec3d q2 = surf->get_position(qtri[2]);

			double T = surf->get_triangle_area(j);

			double* v0 = getKernelGradient(p, q0);
			double* v1 = getKernelGradient(p, q1);
			double* v2 = getKernelGradient(p, q2);

			for (int i=0; i<3; i++)
			{
				for (int j=0; j<3; j++)
				{
					for (int k=0; k<3; k++)
					{
						K[i][j][k] = (v0[getIndex(i,j,k)] + v1[getIndex(i,j,k)] + v2[getIndex(i,j,k)]) * T / 3.0;				
					}
				}
			}

			free(v0);
			free(v1);
			free(v2);

			Vec3d s(theta[3*j+0], theta[3*j+1], theta[3*j+2]);

			Mat33d Gu = tensorVectorContraction(K, s);

			Ft += Gu + Gu.transpose();		
		}

		for (int a=0; a<3; a++)
		{
			for (int b=0; b<3; b++)
			{				
				(*F)[getTensorArrayIndex(a,b,i)] = Ft(a, b);
			}			
		}

	}
}
